// This is for XP
#define WINVER 0x0501 

#include <afxwin.h>
#include <afxsock.h>
#include <windows.h>
#include <math.h>
#include "haptics.h"
#include "WinThread_Server.h"

HANDLE t_WinThread_S = NULL ;
HANDLE t_hEndEvent_S = NULL ;
DWORD WINAPI ThreadFuncS( LPVOID ) ;

CAsyncSocket SerSock;
CAsyncSocket SerSock2;

char RecvBuffSer[1024];
char SendBuffSer[1024];
int SockStatus=0;

class CHaptic:public HapticsClass{};
CHaptic tHaptic;
/******************************************************************************/
BOOL InitWinThread_S( void )
{
	DWORD dwID ;

	// Create the event //
	t_hEndEvent_S = CreateEvent( NULL, TRUE, FALSE, NULL );
	if( !t_hEndEvent_S )
		return FALSE;

	// Create an thread //
	t_WinThread_S = CreateThread( NULL, 0, ThreadFuncS, NULL, 0, &dwID);
	if(!AfxSocketInit()) AfxMessageBox("SOCKETS INIT FAILED");


	return t_WinThread_S != NULL ;
}
void UnInitWinThread_S( void )
{
	SetEvent(t_hEndEvent_S);
	WaitForSingleObject( t_WinThread_S, INFINITE );
	CloseHandle( t_hEndEvent_S );
	CloseHandle( t_WinThread_S );
}

/******************************************************************************/
DWORD WINAPI ThreadFuncS( LPVOID )
{
	tHaptic.init(10);

	// Loop 32 times a second until the g_hEndEvent becomes signalled.
	while( WaitForSingleObject(t_hEndEvent_S,10) != WAIT_OBJECT_0 )
	{
		switch (SockStatus){
		case 0:// Listening //
//			SerSock.Create(3108,SOCK_STREAM, FD_READ | FD_WRITE | FD_OOB | FD_ACCEPT | FD_CONNECT | FD_CLOSE);
			SerSock.Create(4000);
			if (SerSock.Listen(0)) SockStatus=1;
			break; 

		case 1:// Accepting //
			if(SerSock.Accept(SerSock2,NULL,NULL)) SockStatus=2;
			break; 

		case 2:// Connected //

			GetHServo(SendBuffSer);
			SerSock2.Send(&SendBuffSer,30);
			char RecvBuff[50];
			while (strlen(RecvBuff)<29)
			{
				SerSock2.Receive(&RecvBuff,30,MSG_PEEK);
			}

			SerSock2.Receive(&RecvBuffSer,30);
			if(RecvBuffSer[0]=='@')SetHServo(RecvBuffSer);
			break;
		}//switch
	}//while

	SerSock.Close();
	SerSock2.Close();

	return 0 ;
}
/******************************************************************************/
void SetHServo(char posH[1024]){
	if(posH[0]=='@'){
		int i;
		double posC[3];
		char posCC[3][10];
		char * stopstring;
		for (i=0;i<9;i++){ 
			posCC[0][i]=posH[i+1];
			posCC[1][i]=posH[i+11];
			posCC[2][i]=posH[i+21];
			posC[0] = strtod(posCC[0], &stopstring );
			posC[1] = strtod(posCC[1], &stopstring );
			posC[2] = strtod(posCC[2], &stopstring );
		}
		tHaptic.setPosition(posC);
	}
}
void GetHServo(char posH[1024]){
		double pos[3];
		tHaptic.getPosition(pos);
		sprintf(posH,"@%9.4f %9.4f %9.4f",pos[0],pos[1],pos[2]);
}